#!/usr/bin/python
import sys
from time import sleep
from serial import Serial
import pylab

samples = int(sys.argv[1])

imuLabels = ['x','y','z','roll','pitch','yaw']
imu = Serial('/dev/ttyUSB0',115200)
imu.open()
imu.read(5000)
imu.readline()

data = []
while(len(data) < samples):
  imuData = imu.readline().strip().split(',')
  if len(imuData) < 7: continue
  try:
    imuData = [int(value) for value in imuData]
    for value in imuData[1:]: 
      if abs(value) > 1024: raise exception;

    data.append(imuData)
    print imuData

  except:
    continue

sampleNums = [entry[0] for entry in data]

x = [entry[1] for entry in data]
y = [entry[2] for entry in data]
z = [entry[3] for entry in data]
p = [entry[4] for entry in data]
r = [entry[5] for entry in data]
y = [entry[6] for entry in data]

pylab.subplot(2,1,1)
pylab.plot(sampleNums,x,label='x')
pylab.plot(sampleNums,y,label='y')
pylab.plot(sampleNums,z,label='z')
pylab.legend()

pylab.subplot(2,1,2)
pylab.plot(sampleNums,p,label='pitch')
pylab.plot(sampleNums,r,label='roll')
pylab.plot(sampleNums,y,label='yaw')
pylab.legend()

pylab.show()

